/**
 ******************************************************************************
 *
 * @file       UAVTalkAT.ino
 * @author     KipkoOl
 * @brief      Arduino based antenna tracker for UAVTalk protocol.
 * 
 * 	           Most code has been shamelessly stolen from this multiwii project:
 *             http://code.google.com/p/mwp-arduino-antenna-tracker/
 *
 *             UAVTalk arduino api carjacked & stripped down from minoposd: 
 *             http://code.google.com/p/minoposd/wiki/OpenPilot
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/

#define PROTOCOL_UAVTALK


#include <LCD03_I2C.h>
#include <Metro.h>
#include <Servo.h>
#include <EEPROM.h>
#include "EEPROMAnything.h"
#include "UAVTalk.h"

//----------------------------------------------------------------------------------------------
//                                 CONFIGURATION                             


//Telemetry baudrate used
#define TELEMETRY_BAUD  57600

//END OF CONFIG
//---------------------------------------------------------------------------------------------
LCD03_I2C LCD(0x63,20,4);

Servo headingServo;
Servo altangleServo;

boolean Home_Captured = false;
boolean offsetCaptured = false;

int Setup_Field = 1;
String Setup_State = "CONFIG";
char* Setup_Title = "";
String Telemetry_State = "PAGE1";
String Servo_Type = " ";
char* Testservo_Status = "";
int testservo = 1;
int current_H_PWM;
int current_I_PWM;

int current_reading;

long LCD_Timer = 0;
long GPS_Timer = 0;
float Home_altitude = 0;
float Gnd_alt = 0;
float Home_latitude = 0;
float Home_longitude = 0;
int GPS_fix = 0;
float GPS_latitude = 0;
float GPS_longitude = 0;
float GPS_altitude = 0;
float heading = 0;
float hdgOffset = 0;
float distance = 0;
float angle = 0;

//dev temp  To clean up
int tilt_maxangle = 150;
int pan_maxangle = 180;

//

//declare used uavtalk variables
static float        uav_lat = 0;                    // latidude
static float        uav_lon = 0;                    // longitude
static uint8_t      uav_satellites_visible = 0;     // number of satelites
static uint8_t      uav_fix_type = 0;               // GPS lock 0-1=no fix, 2=2D, 3=3D
static float        uav_heading = 0;                // ground course heading from GPS
static float        uav_alt = 0;                    // altitude
static float        uav_groundspeed = 0;            // ground speed

struct config_t
{
  int config_crc;
  int pan_min;
  int pan_mid;
  int pan_max;
  int tilt_min;
  int tilt_mid;
  int tilt_max;
  
} configuration;


void setup() {

  LCD.Clear();
  LCD.Backlight_On();
  LCD.PrintChar(1, 1, "    UAVTalk-at      ");
  LCD.PrintChar(1, 2, "   GhettoStation    ");
  LCD.PrintChar(1, 4, "Init...             ");
  


  Serial.begin(TELEMETRY_BAUD);



  pinMode(3, INPUT);  //+ Button
  pinMode(4, INPUT);  //- Button
  pinMode(5, INPUT);  //Enter Button
  pinMode(13, OUTPUT);  //Feedback LED

  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);
  digitalWrite(5, HIGH);

  //init vars

  GPS_latitude = 0;
  GPS_longitude = 0;
  GPS_altitude = 0;  

  delay(1500);


	// retrieve configuration from EEPROM
	EEPROM_readAnything(0, configuration);
	//use default parameters if config is empty
	if (configuration.config_crc != 1337) {
    
          // clearing eeprom
          for (int i = 0; i < 1024; i++)
            EEPROM.write(i, 0);
    configuration.config_crc = 1337;
	configuration.pan_min = 550;
	configuration.pan_mid = 1450;
	configuration.pan_max = 2350;
	configuration.tilt_min = 620;
	configuration.tilt_mid = 1480;
	configuration.tilt_max = 2330;
        EEPROM_writeAnything(0, configuration);
	}

	
    headingServo.attach(6);
	altangleServo.attach(9);

  headingServo.writeMicroseconds(configuration.pan_mid);
  altangleServo.writeMicroseconds(configuration.tilt_mid);
  

  }

  void loop(){


    
    
    ++GPS_Timer;
    //controls frequency of GPS/servo processing updates 
    if (GPS_Timer == 1000) {

 
	  // Startup Menu selection
	  if (Setup_State == "CONFIG") {

	      current_H_PWM = configuration.pan_mid;
	      current_I_PWM = configuration.tilt_mid;
		  headingServo.writeMicroseconds(current_H_PWM);
		  altangleServo.writeMicroseconds(current_I_PWM);
		  
		if (checkButtonsState(3) == LOW) {
			if (Setup_Field < 4) { 
		   Setup_Field ++;
		   }
		   else {
		   Setup_Field = 1;
		   }
		 
		}
		if (checkButtonsState(4) == LOW) {
			if (Setup_Field > 1) { 
		   Setup_Field --;
		   }
		   else {
		   Setup_Field = 4;
		   }
		}
		if (checkButtonsState(5) == LOW) {
			switch (Setup_Field) {
			case 1: 
				Setup_State = "START";
				break;
			case 2:
				Setup_State = "CONFIG_SERVO";
				Servo_Type = "pan";
				break;
			case 3:
				Setup_State = "CONFIG_SERVO";
				Servo_Type = "tilt";
				break;
			case 4:
				Setup_State = "TEST_SERVO";
				break;
			}
                        cleanScreen();
			Setup_Field = 1;
			delay(500);
		}
	  }
	  // end of menu selection
	  
	  // Servo config
	  
	  if (Setup_State == "CONFIG_SERVO") {


		if (Servo_Type == "pan") {
		
			switch (Setup_Field) {
			case 1:
					if (checkButtonsState(3) == LOW) {
						configuration.pan_min += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.pan_min -= 1;
					}
					headingServo.writeMicroseconds(configuration.pan_min);
					break;
					
			case 2:
					if (checkButtonsState(3) == LOW) {
						configuration.pan_mid += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.pan_mid -= 1;
					}
					headingServo.writeMicroseconds(configuration.pan_mid);
					break;
			case 3:
					if (checkButtonsState(3) == LOW) {
						configuration.pan_max += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.pan_max -= 1;
					}
					headingServo.writeMicroseconds(configuration.pan_max);
					break;
			}
		
		}
		else if (Servo_Type == "tilt") {
			switch (Setup_Field) {
			case 1:
					if (checkButtonsState(3) == LOW) {
						configuration.tilt_min += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.tilt_min -= 1;
					}
					altangleServo.writeMicroseconds(configuration.tilt_min);
					break;
					
			case 2:
					if (checkButtonsState(3) == LOW) {
						configuration.tilt_mid += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.tilt_mid -= 1;
					}
					altangleServo.writeMicroseconds(configuration.tilt_mid);
					break;
			case 3:
					if (checkButtonsState(3) == LOW) {
						configuration.tilt_max += 1;
					}
					if (checkButtonsState(4) == LOW) {
						configuration.tilt_max -= 1;
					}
					altangleServo.writeMicroseconds(configuration.tilt_max);
					break;
			}
		
		}
	  	if (checkButtonsState(5) == LOW) {
				Setup_Field += 1;
				delay (200);
				
				if (Setup_Field >= 4) {
				//store servo configuration to eeprom
				EEPROM_writeAnything(0, configuration);
				Setup_Field = 1;
				Setup_State = "CONFIG";
				cleanScreen();
				}
		}
	  }
	  
	  //end of servo config
	  
	  // servo test
	  if (Setup_State == "TEST_SERVO") {
  
		switch (testservo) {
			case 1:
						Testservo_Status = "Going to Tilt min...";
						if (current_I_PWM > configuration.tilt_min) {
							current_I_PWM -= 10;
							altangleServo.writeMicroseconds(current_I_PWM);
						
						}
						else {
						current_I_PWM = configuration.tilt_min;
						delay (1000);
						testservo = 2;
						}
						break;
			case 2:
						Testservo_Status = "Going to Tilt max...";
						if (current_I_PWM < configuration.tilt_max) {
							current_I_PWM += 10;
							altangleServo.writeMicroseconds(current_I_PWM);

						}
						else 
						{
						current_I_PWM = configuration.tilt_max;
						delay (1000);
						testservo = 3;
						}
						break;
			case 3:
						Testservo_Status = "Going to Tilt mid...";
						if (current_I_PWM > configuration.tilt_mid) {
							current_I_PWM -= 10;
							altangleServo.writeMicroseconds(current_I_PWM);

						}
						else {
						current_I_PWM = configuration.tilt_mid;
						delay(1000);
						testservo = 4;
						}
						break;
			case 4:
						Testservo_Status = "Going to Pan min...";
						if (current_H_PWM > configuration.pan_min) {
							current_H_PWM -= 10;
							headingServo.writeMicroseconds(current_H_PWM);

						}
						else {
						current_I_PWM = configuration.pan_min;
						delay (1000);
						testservo = 5;
						}
						break;
			case 5:
						Testservo_Status = "Going to Pan max...";
						if (current_H_PWM < configuration.pan_max) {
							current_H_PWM += 10;
							headingServo.writeMicroseconds(current_H_PWM);

						}
						else {
						current_I_PWM = configuration.pan_max;
						delay (1000);
						testservo = 6;
						}
						break;
			case 6:
						Testservo_Status = "Going to Pan mid...";
						if (current_H_PWM > configuration.pan_mid){
							current_H_PWM -= 10;
							headingServo.writeMicroseconds(current_H_PWM);

						}
						else {
						current_I_PWM = configuration.pan_mid;
                                                Setup_State = "CONFIG";
                                                testservo=1;
                                                cleanScreen();
                                                }    
						break;
		}
	
            if (checkButtonsState(5) == LOW) {
	      Setup_State = "CONFIG";
            }
            delay(2);
	  }
	  
	  //end of servo test
	  
      if (Setup_State == "START") {
        //read UAVTalk stream
        uavtalk_read();
        if (uav_lat != 0) {
          GPS_latitude = uav_lat;
          GPS_longitude = uav_lon;
          GPS_altitude = uav_alt;

          if (uav_fix_type == 3) {
            Setup_Title = "3D fix. Set home pos";

          }
          else {
            Setup_Title = "Waiting for GPS fix ";
          }
        }
        else {
          Setup_Title = "No GPS data         ";
        }
        //if button pressed, set current LAT LON as home pos
        if ((checkButtonsState(5) == LOW) && (uav_fix_type == 3)) { 
          cleanScreen();
          Home_altitude = uav_alt;
          Home_latitude = uav_lat;
          Home_longitude = uav_lon;
          Setup_Title = "HOME RECORDED";
          //Illuminate LED & wait 3 sec to indicate that the home position has been captured
          digitalWrite(13, HIGH);
          Setup_State = "HOME_SET";
          delay(1000);
          digitalWrite(13, LOW);
        }
      }

      if (Setup_State == "HOME_SET") { 
         //read UAVTalk stream
         uavtalk_read();   
        //uav is moved over 20m directly ahead of tracker zero angle and button pressed to set the offset. 


        if (uav_lat != 0) {
          Setup_Title = "Move UAV 20m ahead  ";
          GPS_latitude = uav_lat;
          GPS_longitude = uav_lon;
          GPS_altitude = uav_alt;
        }

        heading = calc_bearing(Home_latitude,Home_longitude,GPS_latitude,GPS_longitude);
        float turn_angle = 0;

        if ((heading <= 360) && (heading >= 180)) {
          turn_angle = -360 + heading;
        } 
        else {
          turn_angle = heading;
        }   

        //Button press confirms you have moved the drone directly out by 20m or more 
        //and you are ready to start tracking

          if (checkButtonsState(5) == LOW) { 
          cleanScreen();
          hdgOffset = turn_angle;
          Setup_State = "READY";
          //3 sec delay after you hit the button in case it moves
          digitalWrite(13, HIGH);
          delay(1000);
        }
      }

      if (Setup_State == "READY") {   
        //read UAVTalk stream
        uavtalk_read();  
        //Home pos and offset are defined so now the gimbals can be moved.
        if (uav_lat != 0) {
          Setup_Title = "READY TO FLY";
          delay(1000);
          GPS_latitude = uav_lat;
          GPS_longitude = uav_lon;  
          GPS_altitude = uav_alt;
          //changing telemetry page on LCD
          if (checkButtonsState(5) == LOW) {
            if (Telemetry_State == "PAGE1") {
              Telemetry_State = "PAGE2";
              cleanScreen();
            }
            else {
              Telemetry_State = "PAGE1";
              cleanScreen();
            }
        }	

        } 
        //distance drone is from home pos
        distance = calc_dist(Home_latitude,Home_longitude,GPS_latitude,GPS_longitude);

        //inclination calculations

        //calculating relative altitude to ground from gps alt 
        Gnd_alt = GPS_altitude - Home_altitude;
        if (Gnd_alt < 0) {
          Gnd_alt = 0;
        }

        angle = 180 - Inclination(distance,((float)Gnd_alt/100),0,0); 
        float inclination_PWM_amount = 0;
        if (angle > 0) {
          inclination_PWM_amount = (angle / 90) * (configuration.tilt_max - configuration.tilt_mid);
        } 
        else if (angle < 0) {
          inclination_PWM_amount = (angle / 90) * (configuration.tilt_mid - configuration.tilt_min);
        } 

        current_I_PWM = configuration.tilt_mid - (int)inclination_PWM_amount;

        if (current_I_PWM < configuration.tilt_min) {
          current_I_PWM = configuration.tilt_min;
        }

        if (current_I_PWM > configuration.tilt_max) { 
          current_I_PWM = configuration.tilt_max;
        }

        //heading calculations
        heading = calc_bearing(Home_latitude,Home_longitude,GPS_latitude,GPS_longitude);

        float turn_angle = 0;

        if ((heading <= 360) && (heading >= 180)) {
          turn_angle = -360 + heading;
        } 
        else {
          turn_angle = heading;
        }

        float heading_PWM_amount = 0;
        if (turn_angle > 0) {
          heading_PWM_amount = (turn_angle/180)* ((float)configuration.pan_mid - (float)configuration.pan_min);
        } 
        else if (turn_angle < 0) {
          heading_PWM_amount = (turn_angle/180)* ((float)configuration.pan_max - (float)configuration.pan_mid);
        }

        current_H_PWM = configuration.pan_mid - (int)heading_PWM_amount;

        if (current_H_PWM < configuration.pan_min) {
          current_H_PWM = configuration.pan_min;
        }

        if (current_H_PWM > configuration.pan_max) { 
          current_H_PWM = configuration.pan_max;
        }

        //set servo positions
        altangleServo.writeMicroseconds(current_I_PWM); 
        headingServo.writeMicroseconds(current_H_PWM); 

        //adjust heading offset when toggle switch is left or right
        if (checkButtonsState(3) == LOW) {
          hdgOffset += 1;
        }

        if (checkButtonsState(4) == LOW) {
          hdgOffset -= 1;
        }
        
      }

      GPS_Timer=0;
    }


    ++LCD_Timer;
    //controls frequency of LCD updates
    if (LCD_Timer == 2000) {
     
	  if (Setup_State == "CONFIG") {
		LCD.PrintChar(1, 1, "#1 START TRACKING  ");
		LCD.PrintChar(1, 2, "#2 CONFIGURE PAN   ");
		LCD.PrintChar(1, 3, "#3 CONFIGURE TILT  ");
		LCD.PrintChar(1, 4, "#4 TEST SERVOS     ");
		if (Setup_Field == 1) {
			LCD.PrintChar(20, 1, "<");
			LCD.PrintChar(20, 2, " ");
			LCD.PrintChar(20, 3, " ");
			LCD.PrintChar(20, 4, " ");
	   }
	   else if (Setup_Field == 2) {
			LCD.PrintChar(20, 1, " ");
			LCD.PrintChar(20, 2, "<");
			LCD.PrintChar(20, 3, " ");
			LCD.PrintChar(20, 4, " ");
	   }
	   else if (Setup_Field == 3) {
			LCD.PrintChar(20, 1, " ");
			LCD.PrintChar(20, 2, " ");
			LCD.PrintChar(20, 3, "<");
			LCD.PrintChar(20, 4, " ");
	   }
	   else if (Setup_Field == 4) {
			LCD.PrintChar(20, 1, " ");
			LCD.PrintChar(20, 2, " ");
			LCD.PrintChar(20, 3, " ");
			LCD.PrintChar(20, 4, "<");
	   }
	  }
	  
	  
	  if (Setup_State == "CONFIG_SERVO") {
		if (Servo_Type == "pan") {
			LCD.PrintChar(1, 1, "    [PAN SERVO]    ");
			LCD.PrintChar(1, 2, "min: ");
			LCD.PrintChar(9,2, "          ");
			LCD.PrintInt(6, 2, configuration.pan_min, 10);
			LCD.PrintChar(1, 3, "mid:  ");
			LCD.PrintInt(6, 3, configuration.pan_mid, 10);
			LCD.PrintChar(10,3, "         ");
			LCD.PrintChar(1, 4, "max:  ");
			LCD.PrintInt(6, 4, configuration.pan_max, 10);
			LCD.PrintChar(10,3, "         ");
			if (Setup_Field == 1) {
				LCD.PrintChar(20, 2, "<");
				LCD.PrintChar(20, 3, " ");
				LCD.PrintChar(20, 4, " ");
		   }
		   else if (Setup_Field == 2) {
			   LCD.PrintChar(20, 2, " ");
			   LCD.PrintChar(20, 3, "<");
			   LCD.PrintChar(20, 4, " ");
		   }
		   else if (Setup_Field == 3) {
			   LCD.PrintChar(20, 2, " ");
			   LCD.PrintChar(20, 3, " ");
			   LCD.PrintChar(20, 4, "<");
		   }
		}
		else if (Servo_Type == "tilt") {
		
			LCD.PrintChar(1, 1, "    [TILT SERVO]    ");
			LCD.PrintChar(1, 2, "min: ");
			LCD.PrintChar(9,2, "          ");
			LCD.PrintInt(6, 2, configuration.tilt_min, 10);
			LCD.PrintChar(10,2, "         ");
			LCD.PrintChar(1, 3, "mid: ");
			LCD.PrintInt(6, 3, configuration.tilt_mid, 10);
			LCD.PrintChar(10,3, "         ");
			LCD.PrintChar(1, 4, "max: ");
			LCD.PrintInt(6, 4, configuration.tilt_max, 10);
			LCD.PrintChar(10,4, "         ");
			if (Setup_Field == 1) {
				LCD.PrintChar(20, 2, "<");
				LCD.PrintChar(20, 3, " ");
				LCD.PrintChar(20, 4, " ");
		   }
		   else if (Setup_Field == 2) {
			   LCD.PrintChar(20, 2, " ");
			   LCD.PrintChar(20, 3, "<");
			   LCD.PrintChar(20, 4, " ");
		   }
		   else if (Setup_Field == 3) {
			   LCD.PrintChar(20, 2, " ");
			   LCD.PrintChar(20, 3, " ");
			   LCD.PrintChar(20, 4, "<");
		   }
		}
	  }
	  
	  if (Setup_State == "TEST_SERVO") {
  
		LCD.PrintChar(1, 1, "    [SERVO TEST]    ");
		LCD.PrintChar(1, 3, "Tilt:     Pan:      ");
		LCD.PrintChar(1, 2, Testservo_Status);
		LCD.PrintInt(6, 3, current_I_PWM,10);
                LCD.PrintInt(15, 3, current_H_PWM,10);
		LCD.PrintChar(1, 4, "PRESS ENTER TO STOP!");

	  }
	  
	  
      if (Setup_State == "START" ) {
		LCD.PrintChar(1, 1, "Sats: ");
		LCD.PrintInt(6, 1, uav_satellites_visible, 10);
		LCD.PrintChar(7, 1, " Spd:  ");
		LCD.PrintInt(12, 1, uav_groundspeed, 10);
		LCD.PrintChar(14, 1, " FIX:");
		LCD.PrintInt(19 ,1, uav_fix_type, 10);
		LCD.PrintChar(20, 1, " ");
        LCD.PrintChar(1, 2, Setup_Title);
        LCD.PrintChar(1, 3, "LAT  ");
        LCD.PrintFloat(5, 3, GPS_latitude, 10,6);
        LCD.PrintChar(15, 3, "     ");
        LCD.PrintChar(1, 4, "LON   ");
        LCD.PrintFloat(5, 4, GPS_longitude, 10,6);
        LCD.PrintChar(15, 4, "    ");

      } 
      else if (Setup_State == "HOME_SET") {
		LCD.PrintChar(1, 1, "Sats:");
		LCD.PrintInt(6, 1, uav_satellites_visible, 10);
		LCD.PrintChar(7, 1, " Spd:");
		LCD.PrintInt(12, 1, uav_groundspeed, 10);
		LCD.PrintChar(14, 1, " FIX");
		LCD.PrintInt(19 ,1, uav_fix_type, 10);
		LCD.PrintChar(20, 1, " ");
        LCD.PrintChar(1, 2, Setup_Title);
        LCD.PrintChar(1, 4, "DST ");
        LCD.PrintFloat(5, 4, distance, 4,1);
        LCD.PrintChar(9, 4, "m");
        LCD.PrintChar(10, 4, "          ");	
        LCD.PrintChar(1, 3, "                    ");

      } 

      else if (Setup_State == "READY") {
		  LCD.PrintChar(1, 1, "Sats:");
		  LCD.PrintInt(6, 1, uav_satellites_visible, 10);
		  LCD.PrintChar(7, 1, " Spd:");
		  LCD.PrintInt(12, 1, uav_groundspeed, 10);
		  LCD.PrintChar(14, 1, " FIX:");
		  LCD.PrintInt(19 ,1, uav_fix_type, 10);
          LCD.PrintChar(20, 1, " ");
			if (Telemetry_State == "PAGE1") {
			  LCD.PrintChar(1, 2, "ALT ");
			  LCD.PrintFloat(5, 2, Gnd_alt, 4, 1);
			  LCD.PrintChar(9, 2, "m");   
			  LCD.PrintChar(11, 2, "INC ");
			  LCD.PrintFloat(15, 2, angle, 3, 1);
              LCD.PrintChar(18, 2, "   ");
			  LCD.PrintChar(1, 3, "DST ");
			  LCD.PrintFloat(5, 3, distance, 5, 1);
			  LCD.PrintChar(10, 3, "m");
			  LCD.PrintChar(12, 3, "HDG ");
			  LCD.PrintFloat(16, 3, heading, 3, 1);
              LCD.PrintChar(19, 3, "  ");
			  LCD.PrintChar(1, 4, "OFFSET: ");
			  LCD.PrintFloat(8, 4, hdgOffset, 4,1);
			}
			else {
			  LCD.PrintChar(1, 2, "LAT ");
			  LCD.PrintFloat(5, 2, GPS_latitude, 10,6);
              LCD.PrintChar(16, 2, "     ");
			  LCD.PrintChar(1, 3, "LON                 ");
			  LCD.PrintFloat(5, 3, GPS_longitude, 10,6);
              LCD.PrintChar(16, 3, "     ");
		      LCD.PrintChar(1, 4, "OFFSET : ");
			  LCD.PrintFloat(8, 4, hdgOffset, 4,1);
              LCD.PrintChar(16, 4, "     ");

			}
		  }
      LCD_Timer = 0;
    }
  }


float Inclination(float From_X, float From_Y, float To_X, float To_Y) {

  float DistanceX;
  float DistanceY;
  float const NotQuiteZero = 0.00001;
  long const Ninety = 90;
  long const TwoSeventy = 270;
  float const RadsToDegs = 57.29578;

  //Given two points, return the angle of the line from P1 to P2.
  if (From_Y == To_Y)  { 
    To_Y = From_Y - NotQuiteZero; //Prevent div-by-zero error.
  }
  if (To_Y < From_Y) { 
    return ((float)Ninety + atan((To_X - From_X) / (To_Y - From_Y)) * RadsToDegs);
  } 
  else {
    return ((float)TwoSeventy + atan((To_X - From_X) / (To_Y - From_Y)) * RadsToDegs);
  }
}

float calc_bearing(float flat1, float flon1, float flat2, float flon2) {
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1) {
    bear_calc=360+bear_calc; 
  }

  //set offset
  bear_calc = bear_calc - hdgOffset;
  if (bear_calc > 360) {
    bear_calc = bear_calc - 360;
  } 
  else if (bear_calc < 0) {
    bear_calc = bear_calc + 360;
  }

  return bear_calc;
}


float calc_dist(float flat1, float flon1, float flat2, float flon2) {
  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  return (float)sqrt((float)(x*x) + (float)(y*y))*1609.344; 
}

float to_float_6(long value) 
{
  return (float)value/(float)1000000;
}




int checkButtonsState(int pin) {
	int held = 0;
	current_reading = digitalRead(pin);
	while (current_reading == LOW && held < 4)
	{
		delay(100);
		held++;
	}
	return current_reading;
}


void cleanScreen() {
      LCD.PrintChar(1,1, "                    ");
      LCD.PrintChar(1,2, "                    ");
      LCD.PrintChar(1,3, "                    ");
      LCD.PrintChar(1,4, "                    "); 
}

